/*
 * Copyright (c) Huawei Technologies Co., Ltd. 2013-2019.
 * Description: fiq_glue.S
 * Author: xiekunxun <xiekunxun@huawei.com>
 * Create: 2013-09-17
 */

#include <linux/linkage.h>
#include <asm/assembler.h>
#include <asm/unwind.h>

#define STORE_IN_R10	1
		.text
		.global fiq_glue_end
		.global vector_fiq_glue_end

		/* fiq stack: r0-r15,cpsr,spsr of interrupted mode */

#ifdef CONFIG_RTOS_HAL_FIQ_SUPPORT_FOR_THUMB2_KERNEL
.arm
#endif
ENTRY(fiq_glue)
		mrs	r12, spsr
		sub	r11, lr, #4

		/* store regs from previous mode */
		stmfd	sp!, {r11-r12, lr}
		sub	sp, sp, #(7 * 4)
		stmia	sp, {r8-r14}^
		stmfd	sp!, {r0-r7}

                /* get fiq num */
                stmfd   sp!, {r8-r12}
                bl      fiq_get_num
                ldmfd   sp!, {r8-r12}

		/* clean fiq source */
		stmfd	sp!, {r8-r12}
		bl	fiq_clean_source
		ldmfd	sp!, {r8-r12}

		/* jump for reentry */
		subs	r10, #1
		cmp	r10, #0
		bne	nested_fiq

		/* setup function handle's second arguments:previous mode regs */
		mov	r1, sp

		/* trans fiq mode's sp and function handle to previous mode */
		mov	r6, r8
		mov	r7, sp

		/* check previous mode */
		and	r4, r12, #MODE_MASK
		cmp	r4, #USR_MODE
		bne	fiq_from_none_usr_mode

		msr	cpsr_c, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)

		/* setup function handle's third arguments:svc_sp */
		mov	r2, sp

		/* setup function handle's first arguments:reserve */
		sub	r0, r7, #12
		stmfd	r0!, {r2, ip, lr}

		/* call func(data,regs) */
		blx	r6
		sub     r0, r7, #12
		ldmfd	r0, {r2, ip, lr}
		mov	sp, r2
		b	fiq_from_usr_mode_exit

fiq_from_none_usr_mode:
		orr	r4, r4, #(PSR_I_BIT | PSR_F_BIT)
		msr	cpsr_c, r4
		str	sp, [r7, #(4 * 13)]
		str	lr, [r7, #(4 * 14)]
		mrs	r5, spsr
		str	r5, [r7, #(4 * 17)]

		cmp	r4, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)
		/* use fiq stack if we reenter this mode */
		subne	sp, r7, #(4 * 3)

		msr	cpsr_c, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)
#ifdef CONFIG_RTOS_SOFTWARE_CFI
		b	fiq_from_other_mode
#endif
ENTRY(fiq_from_other_mode)
UNWIND(.fnstart)
UNWIND(.save { r0-pc})
		sub	sp, #18*4
		mov	r0, #15*4
1:		ldr	r1, [r7, r0]
		str	r1, [sp, r0]
		subs	r0, #4
		bge	1b

		/* setup function handle's second arguments:previous mode regs */
		mov	r1, r7
		/* setup function handle's third arguments:svc_sp */
		mov	r2, sp
		sub	r0, r7, #12
		stmfd	r0!, {r2, ip, lr}
		/* call func(data,regs) */
		blx	r6
		sub     r0, r7, #12
		ldmfd	r0, {r2, ip, lr}
		mov	sp, r2
		b out_of_region

UNWIND(.fnend)
ENDPROC(fiq_from_other_mode)

out_of_region:
		msr	cpsr_c, r4
		ldr	sp, [r7, #(4 * 13)]
		ldr	lr, [r7, #(4 * 14)]
		msr	spsr_cxsf, r5

fiq_from_usr_mode_exit:
		msr	cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)
		mov	sp, r7
exit_fiq:
		ldmfd	sp!, {r0-r7}
		add	sp, sp, #(7 * 4)
		ldmfd	sp!, {r11-r12, lr}
		add	r10, #1
		msr	spsr_cxsf, r12
		movs	pc, r11

nested_fiq:
		ldmfd	sp!, {r0-r7}
		add	sp, sp, #(7 * 4)
		ldmfd	sp!, {r11-r12, lr}

		orr	r12, r12, #(PSR_F_BIT)
		add	r10, #1
		msr	spsr_cxsf, r12
		movs	pc, r11

fiq_glue_end:
ENDPROC(fiq_glue)


#ifdef CONFIG_RTOS_HAL_FIQ_SUPPORT_FOR_THUMB2_KERNEL
.thumb
#endif
ENTRY(vector_fiq_glue)
#ifdef CONFIG_RTOS_HAL_FIQ_SUPPORT_FOR_THUMB2_KERNEL
.align  5
		adr	r12, 1f
		bx	r12
.arm
1:
#endif
		dsb
                isb
#ifdef CONFIG_RTOS_EARLY_KBOX
		stmfd	sp!, {r8-r10}
		ldr     r8, ekbox_pcontent_addr
		ldr	r9, [r8]
		cmp	r9, #0
		beq	vector_fiq_next
#ifdef CONFIG_SMP
		stmfd sp!, {r0-r3, r12, r14}
		mrc	p15, 0, r0, c0, c0, 5
		ldr r1, mpidr_to_phyid_addr
		blx r1
		mov r8, r0
		ldmfd sp!, {r0-r3, r12, r14}
#else
		mrc	p15, 0, r8, c0, c0, 5
		lsr	r9, r8, #8
		and	r9, r9, #0xf			@get cluster id
		and	r8, r8, #0xf			@get cpu id
		add	r8, r8, r9, lsl #2		@get true cpu id
#endif

		ldr	r9, ekbox_pcontent_addr
		ldr	r10, [r9]
		mov	r9, #0x61			@'a'
		add	r9, r9, r8
		strb	r9, [r10, r8]

vector_fiq_next:
		ldmfd	sp!, {r8-r10}
#endif
                mrc     p15, 0, r12, c1, c0, 0
                isb
                orr     r12, r12, #(1 << 12)		@open I cache
                orr     r12, r12, #(1 << 2)		@open D cache
                isb
                mcr     p15, 0, r12, c1, c0, 0
                dsb
                isb
		mov	pc, r9
#ifdef CONFIG_RTOS_EARLY_KBOX
ekbox_pcontent_addr:
		.word   ekbox_pcontent
#endif
#ifdef CONFIG_SMP
mpidr_to_phyid_addr:
		.word	mpidr_to_phyid
#endif
vector_fiq_glue_end:
ENDPROC(vector_fiq_glue)

#ifdef CONFIG_RTOS_HAL_FIQ_SUPPORT_FOR_THUMB2_KERNEL
.arm
#endif
.align 5
ENTRY(fiq_glue_setup) /* func, data, sp */
		mrs	r3, cpsr
		msr	cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)
		movs	r8, r0
		mov	sp, r2
		ldr	r9, =fiq_glue
		mov	r10, #1
		msr	cpsr_c, r3
		bx	lr
ENDPROC(fiq_glue_setup)


